
#include <bur/plctypes.h>

#ifdef _DEFAULT_INCLUDES
	#include <AsDefault.h>
#endif

void _INIT ProgramInit(void)
{

	// assembly needs to be ready for AsmPowerOn
	// a single shuttle has to be placed in the double comb area of the diverter
	// connect your application-specific assembly and segment references here

	gCalibDiv.AssemblyReference			= (UDINT)&gAssembly_1;
	gCalibDiv.BaseSegmentReference		= (UDINT)&SEG_T1_1;
	gCalibDiv.DivertSegmentReference	= (UDINT)&SEG_T2_1;
	
	
	

	// task init values - do not change ...
	ProcessParID_Init.ParID				= mcACPPAR_ENCOD0_MODE;
	ProcessParID_Init.DataType			= mcACPTRAK_SEG_PARTYPE_UDINT;
	ProcessParID_Init.VariableAddress	= (UDINT)&SegmentPar_ENCOD0_MODE;
	SegmentPar_ENCOD0_MODE = 41;

	ProcessParID_Data[0].ParID				= mcACPPAR_EPROM_PRODUCT_CODE;
	ProcessParID_Data[0].DataType			= mcACPTRAK_SEG_PARTYPE_UDINT;
	ProcessParID_Data[0].VariableAddress	= (UDINT)&SegmentPar_EPROM_PRODUCT_CODE;
	ProcessParID_Data[1].ParID				= mcACPPAR_ENCOD0_POS_ACT;
	ProcessParID_Data[1].DataType			= mcACPTRAK_SEG_PARTYPE_DINT;
	ProcessParID_Data[1].VariableAddress	= (UDINT)&SegmentPar_ENCOD0_POS_ACT;
	ProcessParID_Data[2].ParID				= mcACPPAR_PCTRL_LAG_ERROR;
	ProcessParID_Data[2].DataType			= mcACPTRAK_SEG_PARTYPE_REAL;
	ProcessParID_Data[2].VariableAddress	= (UDINT)&SegmentPar_PCTRL_LAG_ERROR;

	BaseSegmentIndex		= 0;
	DivertingSegmentIndex	= 0;
	
}






void _CYCLIC ProgramCyclic(void)
{

	// Test
	if(BaseSegmentIndex)
	{
		gCalibDiv.BaseSegmentReference = gAssembly.Segment[BaseSegmentIndex].Ref;
	}
	if(DivertingSegmentIndex)
	{
		gCalibDiv.DivertSegmentReference = gAssembly.Segment[DivertingSegmentIndex].Ref;
	}



	switch(Step)
	{
		case 0:
			if(gCalibDiv.Execute)
			{
				Step = 10;
				gCalibDiv.Busy = 1;
			}
		break;
		
		
		// set segment to open loop mode
		case 10:
			MC_BR_SegProcessParID_Init0.Execute = 1;
			MC_BR_SegProcessParID_Init0.ChannelIndex = 0;	
			MC_BR_SegProcessParID_Init0.Mode = mcACPTRAK_SEG_PARID_SET;
			MC_BR_SegProcessParID_Init0.DataAddress = (UDINT)&ProcessParID_Init;
			MC_BR_SegProcessParID_Init0.NumberOfParIDs = 1;
			Step++;
		break;
		case 11:
			if(MC_BR_SegProcessParID_Init0.Done)
				Step++;
			else if(MC_BR_SegProcessParID_Init0.Error)
				Step = 99;
		break;
	
		case 12:
			MC_BR_SegProcessParID_Init1.Execute = 1;
			MC_BR_SegProcessParID_Init1.ChannelIndex = 0;	
			MC_BR_SegProcessParID_Init1.Mode = mcACPTRAK_SEG_PARID_SET;
			MC_BR_SegProcessParID_Init1.DataAddress = (UDINT)&ProcessParID_Init;
			MC_BR_SegProcessParID_Init1.NumberOfParIDs = 1;
			Step++;
		break;
		case 13:
			if(MC_BR_SegProcessParID_Init1.Done)
				Step = 20;
			else if(MC_BR_SegProcessParID_Init1.Error)
				Step = 99;
		break;
		
		
		// assembly power on
		case 20:
			MC_BR_AsmPowerOn_0.Execute = 1;
			Step++;
		break;
		case 21:
			if(MC_BR_AsmPowerOn_0.Done)
				Step = 30;
			else if(MC_BR_AsmPowerOn_0.Error)
				Step = 99;
		break;
		
		
		// wait
		case 30:
			TON_SegPowerOn.IN = 1;
			TON_SegPowerOn.PT = 100;
			if(TON_SegPowerOn.Q)
			{
				TON_SegPowerOn.IN = 0;
				Step = 40;
			}
		break;
		
		
		// read segment / position data
		case 40:
			MC_BR_SegProcessParID_Data0.Execute = 1;
			MC_BR_SegProcessParID_Data0.ChannelIndex = 0;	
			MC_BR_SegProcessParID_Data0.Mode = mcACPTRAK_SEG_PARID_GET;
			MC_BR_SegProcessParID_Data0.DataAddress = (UDINT)&ProcessParID_Data;
			MC_BR_SegProcessParID_Data0.NumberOfParIDs = 3;
			Step++;
		break;
		case 41:
			if(MC_BR_SegProcessParID_Data0.Done)
			{
				gCalibDiv.BaseSegmentProductCode	= SegmentPar_EPROM_PRODUCT_CODE;
				BaseSegmentEncod0Pos				= (LREAL)SegmentPar_ENCOD0_POS_ACT/10000000.0;
				BaseSegmentLagError					= SegmentPar_PCTRL_LAG_ERROR;
				Step++;
			}
			else if(MC_BR_SegProcessParID_Data0.Error)
				Step = 99;
		break;
	
		case 42:
			MC_BR_SegProcessParID_Data1.Execute = 1;
			MC_BR_SegProcessParID_Data1.ChannelIndex = 0;	
			MC_BR_SegProcessParID_Data1.Mode = mcACPTRAK_SEG_PARID_GET;
			MC_BR_SegProcessParID_Data1.DataAddress = (UDINT)&ProcessParID_Data;
			MC_BR_SegProcessParID_Data1.NumberOfParIDs = 3;
			Step++;
		break;
		case 43:
			if(MC_BR_SegProcessParID_Data1.Done)
			{
				gCalibDiv.DivertSegmentProductCode	= SegmentPar_EPROM_PRODUCT_CODE;
				DivertSegmentEncod0Pos				= (LREAL)SegmentPar_ENCOD0_POS_ACT/10000000.0;
				DivertSegmentLagError				= SegmentPar_PCTRL_LAG_ERROR;
				Step = 50;
			}
			else if(MC_BR_SegProcessParID_Data1.Error)
				Step = 99;
		break;
		
		
		// assembly power off
		case 50:
			MC_BR_AsmPowerOff_0.Execute = 1;
			Step++;
		break;
		case 51:
			if(MC_BR_AsmPowerOff_0.Done)
				Step = 60;
			else if(MC_BR_AsmPowerOff_0.Error)
				Step = 99;
		break;
		
		
		// calculate
		case 60:
			switch(gCalibDiv.BaseSegmentProductCode)
			{
				case 0xEBFE:	// PT1 8YFIZ1.AA63.00-1
				case 0xEE0A:	// PT1 8YFIZ1.AA63.10-1
					gCalibDiv.BaseSegmentLength	= 0.630;
				break;
				case 0xEC00:	// PT1 8YFIZ1.AB43.00-1
				case 0xEE0B:	// PT1 8YFIZ1.AB43.10-1
				case 0xEC01:	// PT1 8YFIZ1.BA43.00-1
				case 0xEE0C:	// PT1 8YFIZ1.BA43.10-1
					gCalibDiv.BaseSegmentLength	= 0.43564205646954734519;
				break;
				default:		// unknown
					gCalibDiv.BaseSegmentLength	= 0.0;
//					gCalibDiv.BaseSegmentLength	= 0.630;
				break;
			}
			switch(gCalibDiv.DivertSegmentProductCode)
			{
				case 0xEBFE:	// PT1 8YFIZ1.AA63.00-1
				case 0xEE0A:	// PT1 8YFIZ1.AA63.10-1
					gCalibDiv.DivertSegmentLength	= 0.630;
				break;
				case 0xEC00:	// PT1 8YFIZ1.AB43.00-1
				case 0xEE0B:	// PT1 8YFIZ1.AB43.10-1
				case 0xEC01:	// PT1 8YFIZ1.BA43.00-1
				case 0xEE0C:	// PT1 8YFIZ1.BA43.10-1
					gCalibDiv.DivertSegmentLength	= 0.43564205646954734519;
				break;
				default:		// unknown
					gCalibDiv.DivertSegmentLength	= 0.0;
				break;
			}
			
			if(DivertSegmentEncod0Pos <= (gCalibDiv.DivertSegmentLength/2.0))
			{
				gCalibDiv.DivertSegmentPositionRelToStart	= 0.0;
				gCalibDiv.DivertSegmentPositionRelToEnd		= gCalibDiv.DivertSegmentLength;
				gCalibDiv.BaseSegmentPositionRelToStart		= BaseSegmentEncod0Pos + DivertSegmentEncod0Pos;
			}
			else
			{
				gCalibDiv.DivertSegmentPositionRelToStart	= gCalibDiv.DivertSegmentLength;
				gCalibDiv.DivertSegmentPositionRelToEnd		= 0.0;
				gCalibDiv.BaseSegmentPositionRelToStart		= BaseSegmentEncod0Pos - (gCalibDiv.DivertSegmentLength - DivertSegmentEncod0Pos);
			}
			gCalibDiv.BaseSegmentPositionRelToEnd	= gCalibDiv.BaseSegmentLength - gCalibDiv.BaseSegmentPositionRelToStart;
			gCalibDiv.BaseDivertAdjustError			= BaseSegmentLagError + DivertSegmentLagError;
			
			// check for result valid
			if(	(gCalibDiv.BaseSegmentLength > 0.0) &&
				(gCalibDiv.DivertSegmentLength > 0.0) &&
				(gCalibDiv.BaseSegmentPositionRelToStart >= 0.0) &&
				(gCalibDiv.BaseSegmentPositionRelToEnd <= gCalibDiv.BaseSegmentLength) &&
				(gCalibDiv.DivertSegmentPositionRelToStart >= 0.0) &&
				(gCalibDiv.DivertSegmentPositionRelToEnd <= gCalibDiv.DivertSegmentLength) )
				Step = 100;
			else
				Step = 99;
		
		break;
		
		
		// error
		case 99:
			gCalibDiv.Busy = 0;
			gCalibDiv.Error = 1;
			if(gCalibDiv.Execute == 0)
			{
				gCalibDiv.Error = 0;
				Step = 0;
			}
		break;
		
		
		// done
		case 100:
			gCalibDiv.Busy = 0;
			gCalibDiv.Done = 1;
			if(gCalibDiv.Execute == 0)
			{
				gCalibDiv.Done = 0;
				Step = 0;
			}
		break;
	
	}
	
	MC_BR_AsmPowerOn_0.Assembly = (McAssemblyType*)gCalibDiv.AssemblyReference;
	MC_BR_AsmPowerOn_AcpTrak(&MC_BR_AsmPowerOn_0);
	MC_BR_AsmPowerOn_0.Execute = 0;
	
	MC_BR_AsmPowerOff_0.Assembly = (McAssemblyType*)gCalibDiv.AssemblyReference;
	MC_BR_AsmPowerOff_AcpTrak(&MC_BR_AsmPowerOff_0);
	MC_BR_AsmPowerOff_0.Execute = 0;
	
	TON(&TON_SegPowerOn);
	
	MC_BR_SegProcessParID_Init0.Segment = (McSegmentType*)gCalibDiv.BaseSegmentReference;
	MC_BR_SegProcessParID_AcpTrak(&MC_BR_SegProcessParID_Init0);
	MC_BR_SegProcessParID_Init0.Execute = 0;

	MC_BR_SegProcessParID_Init1.Segment = (McSegmentType*)gCalibDiv.DivertSegmentReference;
	MC_BR_SegProcessParID_AcpTrak(&MC_BR_SegProcessParID_Init1);
	MC_BR_SegProcessParID_Init1.Execute = 0;

	MC_BR_SegProcessParID_Data0.Segment = (McSegmentType*)gCalibDiv.BaseSegmentReference;
	MC_BR_SegProcessParID_AcpTrak(&MC_BR_SegProcessParID_Data0);
	MC_BR_SegProcessParID_Data0.Execute = 0;

	MC_BR_SegProcessParID_Data1.Segment = (McSegmentType*)gCalibDiv.DivertSegmentReference;
	MC_BR_SegProcessParID_AcpTrak(&MC_BR_SegProcessParID_Data1);
	MC_BR_SegProcessParID_Data1.Execute = 0;

}

void _EXIT ProgramExit(void)
{

}

